# 这是旧的电机驱动模块
# motor_control.py
import RPi.GPIO as GPIO
import time
import threading

# 引脚定义
EA, I2, I1, EB, I4, I3, LS, RS = (13, 19, 26, 16, 21, 20, 6, 12)
FREQUENCY = 50

# 初始化GPIO
def init_gpio()-> tuple:
    GPIO.setmode(GPIO.BCM)
    GPIO.setup([EA, I2, I1, EB, I4, I3], GPIO.OUT)
    GPIO.setup([LS, RS], GPIO.IN)
    
    # 初始化PWM
    pwma = GPIO.PWM(EA, FREQUENCY)
    pwmb = GPIO.PWM(EB, FREQUENCY)
    pwma.start(0)
    pwmb.start(0)
    
    return pwma, pwmb

# 速度计数器变量
lspeed = 0
rspeed = 0
lcounter = 0
rcounter = 0

# 编码器回调函数
def encoder_callback(channel)-> None:
    global lcounter, rcounter
    if channel == LS:
        lcounter += 1
    elif channel == RS:
        rcounter += 1

# 速度监测线程函数
def speed_monitor(interval=0.1):
    global rspeed, lspeed, lcounter, rcounter
    GPIO.add_event_detect(LS, GPIO.RISING, callback=encoder_callback)
    GPIO.add_event_detect(RS, GPIO.RISING, callback=encoder_callback)
    
    while True:
        # 计算每秒转速
        rspeed = (rcounter / 585.0) / interval  # 585脉冲/圈
        lspeed = (lcounter / 585.0) / interval
        rcounter = 0
        lcounter = 0
        time.sleep(interval)

# 启动速度监测
def start_speed_monitor():
    thread = threading.Thread(target=speed_monitor)
    thread.daemon = True  # 设为守护线程，主程序结束时自动结束
    thread.start()
    return thread

# 电机控制函数
def set_motor_speed(pwma, pwmb, left_speed, right_speed):
    """
    设置左右电机速度
    
    Args:
        pwma, pwmb: PWM对象
        left_speed: 左电机速度，范围-100到100，负值表示反转
        right_speed: 右电机速度，范围-100到100，负值表示反转
    """
    # 控制左电机方向和速度
    if left_speed >= 0:
        GPIO.output(I1, GPIO.HIGH)
        GPIO.output(I2, GPIO.LOW)
        pwma.ChangeDutyCycle(min(abs(left_speed), 100))
    else:
        GPIO.output(I1, GPIO.LOW)
        GPIO.output(I2, GPIO.HIGH)
        pwma.ChangeDutyCycle(min(abs(left_speed), 100))
    
    # 控制右电机方向和速度
    if right_speed >= 0:
        GPIO.output(I3, GPIO.HIGH)
        GPIO.output(I4, GPIO.LOW)
        pwmb.ChangeDutyCycle(min(abs(right_speed), 100))
    else:
        GPIO.output(I3, GPIO.LOW)
        GPIO.output(I4, GPIO.HIGH)
        pwmb.ChangeDutyCycle(min(abs(right_speed), 100))


class PID:
    """PID Controller"""

    def __init__(self, P=30, I=0.06, D=20, speed=0.3, duty=5): # 上课给的代码中duty就没用到
        self.Kp = P
        self.Ki = I
        self.Kd = D
        self.err_pre = 0
        self.err_last = 0
        self.u = 0
        self.integral = 0
        self.ideal_speed = speed
    
    def update(self, feedback_value):
        self.err_pre = self.ideal_speed - feedback_value
        self.integral += self.err_pre
        self.u = self.Kp*self.err_pre + self.Ki*self.integral + self.Kd*(self.err_pre-self.err_last)
        self.err_last = self.err_pre
        
        if self.u > 100:
            self.u = 100
        elif self.u < 0:
            self.u = 0
        return self.u

    def setKp(self, proportional_gain):
        """Determines how aggressively the PID reacts to the current error with setting Proportional Gain"""
        self.Kp = proportional_gain

    def setKi(self, integral_gain):
        """Determines how aggressively the PID reacts to the current error with setting Integral Gain"""
        self.Ki = integral_gain

    def setKd(self, derivative_gain):
        """Determines how aggressively the PID reacts to the current error with setting Derivative Gain"""
        self.Kd = derivative_gain
        
    def reset(self):
        """重置PID控制器状态"""
        self.err_pre = 0
        self.err_last = 0
        self.integral = 0
        self.u = 0

# 使用PID控制的原地转向函数
def rotate_in_place_with_pid(pwma, pwmb, direction, target_speed=1.5, l_pid=None, r_pid=None):
    """
    使用PID控制器控制小车原地转向
    
    Args:
        pwma, pwmb: PWM对象
        direction: 转向方向，'clockwise'顺时针，'counterclockwise'逆时针
        target_speed: 目标转速
        l_pid, r_pid: 左右轮子的PID控制器，如果为None则创建新的
    
    Returns:
        l_pid, r_pid: 返回使用的PID控制器对象，便于连续调用时保持状态
    """
    global lspeed, rspeed
    
    # 如果没有提供PID控制器，创建新的
    if l_pid is None:
        l_pid = PID(P=38.57, I=0.1, D=70, speed=target_speed)
    if r_pid is None:
        r_pid = PID(P=45, I=0.1, D=70, speed=target_speed)
    
    # 根据方向设置电机转向
    if direction == 'counterclockwise':
        # 左轮正转，右轮反转
        GPIO.output(I1, GPIO.HIGH)
        GPIO.output(I2, GPIO.LOW)
        GPIO.output(I3, GPIO.LOW)
        GPIO.output(I4, GPIO.HIGH)
    elif direction == 'clockwise':
        # 左轮反转，右轮正转
        GPIO.output(I1, GPIO.LOW)
        GPIO.output(I2, GPIO.HIGH)
        GPIO.output(I3, GPIO.HIGH)
        GPIO.output(I4, GPIO.LOW)
    else:
        print("方向参数错误，应为'clockwise'或'counterclockwise'")
        return l_pid, r_pid
    
    # 使用PID控制器更新电机速度
    left_duty = l_pid.update(lspeed)
    right_duty = r_pid.update(rspeed)
    
    pwma.ChangeDutyCycle(left_duty)
    pwmb.ChangeDutyCycle(right_duty)
    
    return l_pid, r_pid

# 改进的直线行驶函数（使用两个PID控制器）
def drive_straight_with_pid(pwma, pwmb, target_speed=1.5, l_pid=None, r_pid=None):
    """
    使用PID控制器控制小车直线行驶
    
    Args:
        pwma, pwmb: PWM对象
        target_speed: 目标速度，正值表示前进，负值表示后退
        l_pid, r_pid: 左右轮子的PID控制器，如果为None则创建新的
    
    Returns:
        l_pid, r_pid: 返回使用的PID控制器对象，便于连续调用时保持状态
    """
    global lspeed, rspeed
    
    # 确定行驶方向
    direction = 1 if target_speed >= 0 else -1
    abs_target_speed = abs(target_speed)
    
    # 如果没有提供PID控制器，创建新的
    if l_pid is None:
        l_pid = PID(P=38.57, I=0.1, D=70, speed=abs_target_speed)
    else:
        # 更新现有PID控制器的目标速度
        l_pid.ideal_speed = abs_target_speed
        
    if r_pid is None:
        r_pid = PID(P=45, I=0.1, D=70, speed=abs_target_speed)
    else:
        # 更新现有PID控制器的目标速度
        r_pid.ideal_speed = abs_target_speed
    
    # 根据方向设置电机转向
    if direction > 0:  # 前进
        GPIO.output(I1, GPIO.HIGH)
        GPIO.output(I2, GPIO.LOW)
        GPIO.output(I3, GPIO.HIGH)
        GPIO.output(I4, GPIO.LOW)
    else:  # 后退
        GPIO.output(I1, GPIO.LOW)
        GPIO.output(I2, GPIO.HIGH)
        GPIO.output(I3, GPIO.LOW)
        GPIO.output(I4, GPIO.HIGH)
    
    # 使用PID控制器更新电机速度
    left_duty = l_pid.update(lspeed)
    right_duty = r_pid.update(rspeed)
    
    pwma.ChangeDutyCycle(left_duty)
    pwmb.ChangeDutyCycle(right_duty)
    
    return l_pid, r_pid

# 基于颜色检测的直线行驶函数（使用PID控制）
def drive_straight_with_color(pwma, pwmb, color_offset, target_speed=1.5, l_pid=None, r_pid=None,offset_factor = 0.2):
    """
    根据颜色检测的偏移量控制小车直线行驶，使用PID控制
    
    Args:
        pwma, pwmb: PWM对象
        color_offset: 颜色中心相对于画面中心的偏移量
        target_speed: 目标速度
        l_pid, r_pid: 左右轮子的PID控制器，如果为None则创建新的
    
    Returns:
        l_pid, r_pid: 返回使用的PID控制器对象，便于连续调用时保持状态
    """
    global lspeed, rspeed
    
    # 如果没有提供PID控制器，创建新的
    if l_pid is None:
        l_pid = PID(P=30, I=0.06, D=20, speed=target_speed)
    if r_pid is None:
        r_pid = PID(P=40, I=0.01, D=23, speed=target_speed)
    
    # 根据偏移量调整目标速度
    # 偏移量为正表示目标在右侧，需要向右转，即减小右轮速度
    # 偏移量为负表示目标在左侧，需要向左转，即减小左轮速度
    # 偏移量影响因子为offset_factor，范围0到1
    
    left_target = target_speed
    right_target = target_speed
    
    if color_offset > 0:
        right_target = target_speed * (1 - offset_factor * abs(color_offset))
    else:
        left_target = target_speed * (1 - offset_factor * abs(color_offset))
    
    # 更新PID控制器的目标速度
    l_pid.ideal_speed = left_target
    r_pid.ideal_speed = right_target
    
    # 设置电机方向为正转
    GPIO.output(I1, GPIO.HIGH)
    GPIO.output(I2, GPIO.LOW)
    GPIO.output(I3, GPIO.HIGH)
    GPIO.output(I4, GPIO.LOW)
    
    # 使用PID控制器更新电机速度
    left_duty = l_pid.update(lspeed)
    right_duty = r_pid.update(rspeed)
    
    pwma.ChangeDutyCycle(left_duty)
    pwmb.ChangeDutyCycle(right_duty)
    
    return l_pid, r_pid

# 清理函数
def cleanup(pwma, pwmb):
    pwma.stop()
    pwmb.stop()
    GPIO.cleanup()

# ===== 测试代码 =====
if __name__ == "__main__":
    try:
        # 初始化GPIO和PWM
        pwma, pwmb = init_gpio()
        
        # 启动速度监测线程
        monitor_thread = start_speed_monitor()
        
        # 初始化PID控制器
        l_pid = None
        r_pid = None
        
        # 默认参数
        target_speed = 0.3
        color_offset = 0
        offset_factor = 0.2
        
        print("=== 电机控制测试程序 ===")
        print("按键说明:")
        print("w: 直线前进 (drive_straight_with_pid)")
        print("s: 颜色跟踪 (drive_straight_with_color)")
        print("a: 逆时针旋转")
        print("d: 顺时针旋转")
        print("q: 停止电机")
        print("r: 重置PID控制器")
        print("+-: 调整目标速度 (当前: {:.1f})".format(target_speed))
        print("o: 设置颜色偏移量 (当前: {})".format(color_offset))
        print("f: 调整偏移因子 (当前: {:.1f})".format(offset_factor))
        print("x: 退出程序")
        
        # 主循环
        while True:
            # 显示当前速度
            print("\r左轮速度: {:.2f} 右轮速度: {:.2f} 目标: {:.2f} 偏移: {}".format(
                lspeed, rspeed, target_speed, color_offset), end="")
            
            # 获取键盘输入
            cmd = input("\n请输入命令: ").strip().lower()
            
            if cmd == 'x':
                print("退出程序...")
                break
                
            elif cmd == 'w':
                l_pid, r_pid = None, None
                print("直线前进 (drive_straight_with_pid)")
                l_pid, r_pid = drive_straight_with_pid(pwma, pwmb, target_speed, l_pid, r_pid)
                
            elif cmd == 's':
                l_pid, r_pid = None, None
                print("颜色跟踪 (drive_straight_with_color), 偏移量: {}".format(color_offset))
                l_pid, r_pid = drive_straight_with_color(pwma, pwmb, color_offset, target_speed, l_pid, r_pid, offset_factor)
                
            elif cmd == 'a':
                l_pid, r_pid = None, None
                print("逆时针旋转")
                l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'counterclockwise', target_speed, l_pid, r_pid)
                
            elif cmd == 'd':
                l_pid, r_pid = None, None
                print("顺时针旋转")
                l_pid, r_pid = rotate_in_place_with_pid(pwma, pwmb, 'clockwise', target_speed, l_pid, r_pid)
                
            elif cmd == 'q':
                print("停止电机")
                set_motor_speed(pwma, pwmb, 0, 0)
                
            elif cmd == 'r':
                print("重置PID控制器")
                if l_pid:
                    l_pid.reset()
                if r_pid:
                    r_pid.reset()
                
            elif cmd == '+':
                target_speed = min(1.0, target_speed + 0.1)
                print("目标速度增加到: {:.1f}".format(target_speed))
                
            elif cmd == '-':
                target_speed = max(0.1, target_speed - 0.1)
                print("目标速度减小到: {:.1f}".format(target_speed))
                
            elif cmd == 'o':
                try:
                    new_offset = float(input("请输入新的颜色偏移量 (-200 到 200): "))
                    if -200 <= new_offset <= 200:
                        color_offset = new_offset
                        print("颜色偏移量设置为: {}".format(color_offset))
                    else:
                        print("偏移量超出范围，应在 -200 到 200 之间")
                except ValueError:
                    print("输入无效，请输入一个数字")
                    
            elif cmd == 'f':
                try:
                    new_factor = float(input("请输入新的偏移因子 (0.0 到 1.0): "))
                    if 0.0 <= new_factor <= 1.0:
                        offset_factor = new_factor
                        print("偏移因子设置为: {:.1f}".format(offset_factor))
                    else:
                        print("偏移因子超出范围，应在 0.0 到 1.0 之间")
                except ValueError:
                    print("输入无效，请输入一个数字")
                    
            else:
                print("未知命令: {}".format(cmd))
            
            # 短暂延时，避免CPU占用过高
            time.sleep(0.1)
            
    except KeyboardInterrupt:
        print("\n程序被用户中断")
    finally:
        # 停止电机
        set_motor_speed(pwma, pwmb, 0, 0)
        # 清理资源
        cleanup(pwma, pwmb)
        print("资源已清理")